using System.Collections.Generic;
using UnityEngine;

public class BezierUtils
{
    private static float curveMaxLength = 2.5f;
    public static List<Vector3> GetMarchPointList(List<Vector3> list, int nextPoint)
    {
        var pathList = new List<Vector3>();
        int index = list.Count - 1;
        int smooth = 10;
        List<List<Vector3>> subCurve = new List<List<Vector3>>();
        var tempList = new List<Vector3>();
        //先根据路点之间的距离，把路点分割成若干段
        while (index >= nextPoint - 1 && index >= 0)
        {
            Vector3 curPoint = list[index];
            if (tempList.Count == 0)
            {
                tempList.Add(list[index]);
                tempList.Add(list[index - 1]);
                subCurve.Add(tempList);
                tempList = new List<Vector3>();
                tempList.Add(list[index - 1]);
                index--;
            }
            else
            {
                Vector3 lastPoint = tempList[tempList.Count - 1];
                float distance = Vector3.Distance(lastPoint, curPoint);
                if (distance >= curveMaxLength)
                {
                    //上一段曲线结束
                    subCurve.Add(tempList);
                    //重新开一个直线，连接上个点和这个点
                    tempList = new List<Vector3>();
                    tempList.Add(lastPoint);
                    tempList.Add(curPoint);
                    subCurve.Add(tempList);
                    //以这个点为起点，开启下一个曲线
                    tempList = new List<Vector3>();
                    tempList.Add(curPoint);
                }
                else
                {
                    tempList.Add(curPoint);
                }

                if (tempList.Count > 2)
                {
                    subCurve.Add(tempList);
                    tempList = new List<Vector3>();
                    tempList.Add(curPoint);
                }
            }
            index--;
        }

        if (tempList.Count > 0)
        {
            subCurve.Add(tempList);
        }
        
        for (int i = 0; i < subCurve.Count; i++)
        {
            if (subCurve[i].Count <= 2)
            {
                pathList.AddRange(subCurve[i]);
            }
            else
            {
                pathList.AddRange(GetBezierList(subCurve[i]));
            }
        }
        pathList.Reverse();
        return pathList;
    }


    // list 为需要贝塞尔曲线的点列表，smooth为平滑程度
    // 用例：GetBezierList(PointList);GetBezierList(PointList,100)
    public static List<Vector3> GetBezierList(List<Vector3> list, int smooth = 30)
    {
        //        Stopwatch sw = new Stopwatch();
        //        sw.Start();
        var bList = new List<Vector3>();
        var controlList = GetControlPointNew(list);
        for (int i = 0; i < smooth; i++)
        {
            bList.Add(GetBezier(controlList, i / (float)smooth));
        }
        //        sw.Stop();
        //        Debug.Log($"total: {sw.ElapsedMilliseconds} ms");
        return bList;
    }
    
    private static Vector3 GetBezier(List<Vector3> pList, float t)
    {
        if (pList.Count < 2)
        {
            return pList[0];
        }
        List<Vector3> nList = new List<Vector3>();
        int len = pList.Count - 1;
        for (int i = 0; i < len; i++)
        {
            Vector3 pp = (1 - t) * pList[i] + t * pList[i + 1];
            nList.Add(pp);
        }
        return GetBezier(nList, t);
    }

    public static List<Vector3> GetControlPoint(List<Vector3> pathList)
    {
        if (pathList.Count < 2)
        {
            return pathList;
        }
        List<Vector3> list = new List<Vector3>();
        list.Add(pathList[0]);
        int index = 0;
        while (index < pathList.Count - 2)
        {
            Vector3 p1 = pathList[index];
            Vector3 p2 = pathList[index + 1];
            Vector3 p3 = pathList[index + 2];
            Vector3 a = p2 - p1;
            Vector3 b = p2 - p3;
            float angle = Vector3.Angle(a, b);
            Vector3 ar = Quaternion.AngleAxis((180-angle) / 2,Vector3.up)*a;
            Vector3 br = Quaternion.AngleAxis((180-angle) / -2,Vector3.up)*b;
            float ratio = 0.5f;//0.1~0.5
            Vector3 c1 = br * ratio + p2;
            Vector3 c2 = ar * ratio + p2;
        
            list.Add(c1);
            list.Add(c2);
            index += 2;
        }
        list.Add(pathList[pathList.Count - 1]);
        return list;
    }

    public static List<Vector3> GetControlPointNew(List<Vector3> pathList)
    {
        if (pathList.Count <= 2)
        {
            return pathList;
        }
        List<Vector3> list = new List<Vector3>();
        list.Add(pathList[0]);
        int index = 0;
        while (index < pathList.Count - 2)
        {
            Vector3 p0 = pathList[index];
            Vector3 p1 = pathList[index + 1];
            Vector3 p2 = pathList[index + 2];
            Vector3 a = p0 - p1;
            Vector3 b = p2 - p1;
            Vector3 pc = p1 - Mathf.Sqrt(a.magnitude * b.magnitude) * 0.5f * (a.normalized + b.normalized);
            Vector3 pc1 = p0 / 3 + pc / 3 * 2;
            Vector3 pc2 = pc / 3 * 2 + p2 / 3; 
            list.Add(pc1);
            list.Add(pc2);
            index = index + 2;
        }
        list.Add(pathList[pathList.Count - 1]);
        return list;
    }
    
    public static List<Vector3> GetMorePoint(List<Vector3> pathList)
    {
        if (pathList.Count < 2)
        {
            return pathList;
        }
        List<Vector3> list = new List<Vector3>();
        list.Add(pathList[0]);
        int index = 0;
        while (index < pathList.Count - 2)
        {
            Vector3 p1 = pathList[index];
            Vector3 p2 = pathList[index + 1];
            Vector3 p3 = pathList[index + 2];
            Vector3 a = p1 - p2;
            Vector3 b = p3 - p2;
            float angle = (180 - Vector3.Angle(a, b))/4;
            Vector3 ar = Quaternion.AngleAxis(angle, Vector3.up) * a / (Mathf.Cos(angle * Mathf.Deg2Rad)) / 2; 
            Vector3 br = Quaternion.AngleAxis(-angle, Vector3.up) * b / (Mathf.Cos(angle * Mathf.Deg2Rad)) / 2; 
            Vector3 c1 = p2 + ar;
            Vector3 c2 = p2 + br;
            list.Add(p1);
            list.Add(c1);
            list.Add(p2);
            list.Add(c2);
            index += 2;
        }
        list.Add(pathList[pathList.Count - 1]);
        return list;
    }
    
    public static List<Vector3> GetCatmulRomPath(List<Vector3> list)
    {
        if (list.Count <= 2)
        {
            return list;
        }
        List<Vector3> pathList = new List<Vector3>();
        pathList.Add(list[0]);
        for (int i = 0; i < list.Count - 1; i++)
        {
            Vector3 p0 = list[i];
            if (i > 0)
            {
                p0 = list[i - 1];
            }
            Vector3 p1 = list[i];
            Vector3 p2 = list[i + 1];
            Vector3 p3 = list[i + 1];
            if (i < list.Count - 2)
            {
                p3 = list[i + 2];
            }
            int num = 100;
            for (int j = 0; j < num; j++)
            {
                pathList.Add(CatmullRomPoint(p0,p1,p2,p3,(float)j / num));
            }
        }
        pathList.Add(list[list.Count - 1]);
        return pathList;
    }
    
    private static Vector3 CatmullRomPoint(Vector3 p0, Vector3 p1, Vector3 p2, Vector3 p3, float t)
    {
        return p1 + (0.5f * (p2 - p0) * t) + 0.5f * (2f * p0 - 5f * p1 + 4f * p2 - p3) * t * t +
               0.5f * (-p0 + 3f * p1 - 3f * p2 + p3) * t * t * t;
    }
}

